/*
 *  consdis.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 26/08/2011
 *
 */

#include "consdis.h"

Consdis::Consdis()
:	Constraint()
{}

// ***

Consdis::Consdis(vector<string>& mn, int di)
:	Constraint(mn,di)
{
	initialDis = -1.0;
	r.resize(di);
	isQuadratic = false;
	isScalar = true;
	A.resize(dim,dim);
}

// ***

Consdis::Consdis(vector<string>& mn, int di, double penal)
:	Constraint(mn,di)
{
	initialDis = -1.0;
	r.resize(di);
	isQuadratic = false;
	isScalar = true;
	A.resize(dim,dim);
	penalty = penal;
}

// ***

void Consdis::calcPhi(const VectorXd & Q)
{
	
	// Phi = r - r0
	if (initialDis==-1)	initialDis = lTwoNorm(mPoints[pointNames[0]]->r, mPoints[pointNames[1]]->r);
	r = Q.segment(numDofsPoints[0],dim) - Q.head(dim);
	Phi = r.norm() - initialDis;

	/*
	// Phi = r^2 - r0^2
	if (initialDis==-1)	initialDis = lTwoNorm(mPoints[pointNames[0]]->r, mPoints[pointNames[1]]->r);
	 r = Q.segment(numDofsPoints[0],dim) - Q.head(dim);
	
	Phi = r.squaredNorm() - initialDis*initialDis;
	*/
}

// ***

void Consdis::calcDPhi(const VectorXd & Q)
{
	
	// Phi = r - r0
	r = Q.segment(numDofsPoints[0],dim) - Q.head(dim);
	
	// All terms are needed in case a velocity projection is performed
	dPhi.head(dim) = - r/r.norm();	
	dPhi.segment(numDofsPoints[0],dim) = -dPhi.head(dim);
	
	/*
	// Phi = r^2 - r0^2
 	r = Q.segment(numDofsPoints[0],dim) - Q.head(dim);
	dPhi.head(dim) = - 2*r;
	dPhi.tail(dim) = -dPhi.head(dim);
	*/
}

// ***

void Consdis::calcDDPhi(const VectorXd & Q)
{
	
	// Phi = r - r0
	double length;
	
	//r = Q.tail(dim)-Q.head(dim);
	r = Q.segment(numDofsPoints[0],dim) - Q.head(dim);
	length = r.norm();
	
	A = ( MatrixXd::Identity(dim,dim) - (r*r.transpose())/length/length ) / length;
	
	// I need to define just one block
	ddPhi.block(0,0,dim,dim) = A;
	
//	ddPhi.block(numDofsPoints[0],numDofsPoints[0],dim,dim) = A;
//	ddPhi.block(0,numDofsPoints[0],dim,dim) = -A;
//	ddPhi.block(numDofsPoints[0],0,dim,dim) = -A;	
	
	/*
	// Phi = r^2 - r0^2
	A = 2.0 * MatrixXd::Identity(dim,dim);
	
	*/
}

// ***

void Consdis::setFqSymmetries()
{
	//Fq.head(dim) = FqPenalty.head(dim) - FqLag.head(dim);
	Fq.segment(numDofsPoints[0],dim) =  -Fq.head(dim);
	//Fq.tail(dim) = -Fq.head(dim);	
}

// ***

void Consdis::setKqSymmetries()
{
	// Force block structure
	Kq.block(numDofsPoints[0],numDofsPoints[0],dim,dim) = Kq.block(0,0,dim,dim);
	Kq.block(0,numDofsPoints[0],dim,dim) = -Kq.block(0,0,dim,dim);
	Kq.block(numDofsPoints[0],0,dim,dim) = -Kq.block(0,0,dim,dim);
}

// ***

double Consdis::getScalar(const VectorXd & Q)
{
	return (Q.segment(numDofsPoints[0],dim) - Q.head(dim)).norm();
}

// ***

VectorXd Consdis::getDScalar(const VectorXd & Q)
{
	// Gradient (relative to the coordinates) of the scalar variable.
	
	if (dr.size()==0)	dr.resize(numDofs);
	r = Q.segment(numDofsPoints[0],dim) - Q.head(dim);
	dr.head(dim) = - r/r.norm();	
	dr.segment(numDofsPoints[0],dim) = -dr.head(dim);
	return dr;
}

// ***

MatrixXd Consdis::getDDScalar(const VectorXd & Q)
{
	// Second derivative (relative to coordinates) of the scalar variable
	if (ddr.size()==0)	ddr.resize(numDofs,numDofs);
	r = Q.segment(numDofsPoints[0],dim) - Q.head(dim);
	// Define just one block
	ddr.block(0,0,dim,dim) = -(MatrixXd::Identity(dim,dim) - (r*r.transpose())/r.norm()/r.norm() ) / r.norm();
	
	ddr.block(numDofsPoints[0],numDofsPoints[0],dim,dim) = ddr.block(0,0,dim,dim);
	ddr.block(0,numDofsPoints[0],dim,dim) = -ddr.block(0,0,dim,dim);
	ddr.block(numDofsPoints[0],0,dim,dim) = -ddr.block(0,0,dim,dim);
	return ddr;
}
	
// ***

void Consdis::setParamAtCons(vector<double> &par)
{
	if (par[0]!=0.0) penalty = par[0];
}
